/*
 *    Copyright (c) 2014 Xiang Xu <xuxiang@mail.bnu.edu.cn>
 *
 *    Permission is hereby granted, free of charge, to any person
 *    obtaining a copy of this software and associated documentation
 *    files (the "Software"), to deal in the Software without
 *    restriction, including without limitation the rights to use,
 *    copy, modify, merge, publish, distribute, sublicense, and/or sell
 *    copies of the Software, and to permit persons to whom the
 *    Software is furnished to do so, subject to the following
 *    conditions:
 *
 *    The above copyright notice and this permission notice shall be
 *    included in all copies or substantial portions of the Software.
 *
 *    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 *    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 *    OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 *    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 *    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 *    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 *    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 *    OTHER DEALINGS IN THE SOFTWARE.
 */
#include "arguments.h"
#include "pclwraper.h"

#include <pcl/search/kdtree.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/common.h>

#include <fstream>
#include <iostream>
#include <climits>

// query this after executing a function
Status status;

// organize variables
boost::shared_ptr<Arguments> arguments = boost::shared_ptr<Arguments> (new Arguments);

// original facade clouds
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

// cloud viewer
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;

// search tree
pcl::search::KdTree<pcl::PointXYZ> search;

// We only allow filter once
bool filter = true;

// filter boundary
double xmin = std::numeric_limits<double>::min();
double xmax = -xmin;
double ymin = xmin;
double ymax = -ymin;

// call back functions for viewer
void pp_callback(const pcl::visualization::PointPickingEvent& event, void* cookie);
void FindPickedPoint(const pcl::visualization::PointPickingEvent& event);
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void* cookie);

// setup viewer
void ShowCloud();

int main(int argc, char** argv)
{
    // parse parameters
    status = arguments->Parse(argc, argv);

    if(!status.IsOk())
    {
        cout << status;
        exit(-1);
    }
    else
    {
        cout << "Argments Parsed success.\n";
    }

    // load point cloud
    PclWraper::LoadPCD(arguments->GetFacadePlaneTransformedFile(), cloud);

    // show cloud and into event loop ...
    ShowCloud();

    return 0;
}

void ShowCloud()
{
    viewer.reset(new pcl::visualization::PCLVisualizer());
    viewer->addPointCloud<pcl::PointXYZ> (cloud, "build plane cloud");
    viewer->setBackgroundColor(0, 0, 0);
    viewer->registerPointPickingCallback(&pp_callback);
    viewer->registerKeyboardCallback(&keyboardEventOccurred);
    viewer->spin();
}

void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void* cookie)
{
    if(event.getKeySym() == "f" && filter)
    {
        std::cout << "original #points = " << cloud->points.size() <<  std::endl;
        cout << "Filter x axe ... \n";

        pcl::PointCloud<pcl::PointXYZ>::Ptr x_filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        PclWraper::PassThroughFilter(cloud, x_filtered_cloud, "x", xmin, xmax, true);
        std::cout << "now #points = " << x_filtered_cloud->points.size() <<  std::endl;

        cout << "Filter y axe ...\n";
        pcl::PointCloud<pcl::PointXYZ>::Ptr xy_filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        PclWraper::PassThroughFilter(x_filtered_cloud, xy_filtered_cloud, "y", ymin, ymax, true);
        std::cout << "now #points = " << xy_filtered_cloud->points.size() <<  std::endl;
        cout << "Filter Done.\n";

        cout << "Save filtered clouds ...\n";
        PclWraper::SavePCD(arguments->GetFacadePlaneTransformedFile(), xy_filtered_cloud);
        cout << "Save Done.\n";

        pcl::PointXYZ min_extent, max_extent;
        pcl::getMinMax3D(*xy_filtered_cloud, min_extent, max_extent);

        // save facade scale
        arguments->SaveSpatialScale(min_extent.x, min_extent.y, min_extent.z, max_extent.x,
                                    max_extent.y, max_extent.z);

        filter = false;
    }
}

void FindPickedPoint(const pcl::visualization::PointPickingEvent& event)
{
    int idx = event.getPointIndex();

    if(idx == -1)
    {
        std::cout << "Invalid pick!\n;";
        return;
    }

    search.setInputCloud(cloud);

    // Return the correct index in the cloud instead of the index on the screen
    std::vector<int> indices(1);
    std::vector<float> distances(1);

    // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence,
    // so we must search for the real point
    pcl::PointXYZ picked_pt;
    event.getPoint(picked_pt.x, picked_pt.y, picked_pt.z);
    search.nearestKSearch(picked_pt, 1, indices, distances);

    xmin = xmin > picked_pt.x ? picked_pt.x : xmin;
    xmax = xmax < picked_pt.x ? picked_pt.x : xmax;
    ymin = ymin > picked_pt.y ? picked_pt.y : ymin;
    ymax = ymax < picked_pt.y ? picked_pt.y : ymax;
}

void pp_callback(const pcl::visualization::PointPickingEvent& event, void* cookie)
{
    FindPickedPoint(event);
    cout << "xmin: " << xmin << "\txmax: " << xmax << std::endl;
    cout << "ymin: " << ymin << "\tymax: " << ymax << std::endl;
}



